using System;
using System.Collections.Generic;
using System.Text;
using System.Diagnostics;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework.Physics.CollisionDetectors
{
	/// <summary>
	/// Detects collisions between a <see cref="CollisionBox"/> and a <see cref="CollisionSphere"/>.
	/// </summary>
	public class BoxSphereCollisionDetector : CollisionDetector
	{
		#region CollisionDetector Members

		public override void Detect(CollisionShape shape0, CollisionShape shape1, CollisionContext context)
		{
			// Typecast the shapes to their correct subclasses.
			CollisionBox box = (CollisionBox)shape0;
			CollisionSphere sphere = (CollisionSphere)shape1;

			// Get radius totals.
			float radiusTotal = sphere.Radius + context.Tolerance;

			// Transform the position of the sphere relative to the box.
#if QUATERNION_ORIENTATION
			Vector3 position = Vector3.Transform(sphere.Position - box.Position, Quaternion.Inverse(box.Orientation));
#else
#if INLINE
#if XBOX
			Vector3 position = Vector3.Zero;
#else
			Vector3 position;
#endif
			float x = sphere.Position.X - box.Position.X;
			float y = sphere.Position.Y - box.Position.Y;
			float z = sphere.Position.Z - box.Position.Z;

			position.X = x * box.Orientation.M11 + y * box.Orientation.M12 + z * box.Orientation.M13;
			position.Y = x * box.Orientation.M21 + y * box.Orientation.M22 + z * box.Orientation.M23;
			position.Z = x * box.Orientation.M31 + y * box.Orientation.M32 + z * box.Orientation.M33;
#elif REFERENCE
#if XBOX
			Vector3 position = Vector3.Zero;
#else
			Vector3 position;
#endif
			Vector3.Subtract(ref sphere.Position, ref box.Position, out position);

			Matrix temp;
			Matrix.Transpose(ref box.Orientation, out temp);
			Vector3.TransformNormal(ref position, ref temp, out position);
#else
			Vector3 position = Vector3.TransformNormal(sphere.Position - box.Position, Matrix.Transpose(box.Orientation));
#endif
#endif
			// Determine if a collision is possible with broad-phase check.
			if (Math.Abs(position.X) - radiusTotal > box.Extents.X ||
				Math.Abs(position.Y) - radiusTotal > box.Extents.Y ||
				Math.Abs(position.Z) - radiusTotal > box.Extents.Z)
			{
				return;
			}

			// Clamp each vector component of the sphere's relative position to the box's extents.
			Vector3 collisionPosition = new Vector3(
				MathHelper.Clamp(position.X, -box.Extents.X, box.Extents.X),
				MathHelper.Clamp(position.Y, -box.Extents.Y, box.Extents.Y),
				MathHelper.Clamp(position.Z, -box.Extents.Z, box.Extents.Z));

			// Determine the distance between the clamped position of sphere and sphere's relative position to the box, squared.
#if QUATERNION_ORIENTATION
			float distanceSquared = Vector3.DistanceSquared(collisionPosition, position);
#else
#if INLINE
			x = collisionPosition.X - position.X;
			y = collisionPosition.Y - position.Y;
			z = collisionPosition.Z - position.Z;
			float distanceSquared = x * x + y * y + z * z;
#elif REFERENCE
			float distanceSquared;
			Vector3.DistanceSquared(ref collisionPosition, ref position, out distanceSquared);
#else
			float distanceSquared = Vector3.DistanceSquared(collisionPosition, position);
#endif
#endif
			// Determine if the sphere is close enough to the box for a collision.
			if (distanceSquared > radiusTotal * radiusTotal)
			{
				return;
			}

			// Transform the closest point to the box's local coordinates.
#if QUATERNION_ORIENTATION
			collisionPosition = box.Position + Vector3.Transform(collisionPosition, box.Orientation);
#else
#if INLINE
			x = box.Position.X + (collisionPosition.X * box.Orientation.M11 + collisionPosition.Y * box.Orientation.M21 + collisionPosition.Z * box.Orientation.M31);
			y = box.Position.Y + (collisionPosition.X * box.Orientation.M12 + collisionPosition.Y * box.Orientation.M22 + collisionPosition.Z * box.Orientation.M32);
			z = box.Position.Z + (collisionPosition.X * box.Orientation.M13 + collisionPosition.Y * box.Orientation.M23 + collisionPosition.Z * box.Orientation.M33);
#elif REFERENCE
			Vector3.TransformNormal(ref collisionPosition, ref box.Orientation, out collisionPosition);
			Vector3.Add(ref box.Position, ref collisionPosition, out collisionPosition);
#else
			collisionPosition = box.Position + Vector3.TransformNormal(collisionPosition, box.Orientation);
#endif
#endif
			// Determine the normal of the collision.
#if QUATERNION_ORIENTATION
			Vector3 collisionNormal = collisionPosition - sphere.Position;
#else
#if INLINE
#if XBOX
			Vector3 collisionNormal = Vector3.Zero;
#else
			Vector3 collisionNormal;
#endif
			collisionNormal.X = x - sphere.Position.X;
			collisionNormal.Y = y - sphere.Position.Y;
			collisionNormal.Z = z - sphere.Position.Z;
#elif REFERENCE
#if XBOX
			Vector3 collisionNormal = Vector3.Zero;
#else
			Vector3 collisionNormal;
#endif
			Vector3.Subtract(ref collisionPosition, ref sphere.Position, out collisionNormal);
#else
			Vector3 collisionNormal = collisionPosition - sphere.Position;
#endif
#endif
			Vector3Extension.SafeNormalize(ref collisionNormal);

			// Calculate the depth of the penetration due to the collision.
			float collisionDepth = sphere.Radius - (float)Math.Sqrt(distanceSquared);

			// Create the collision point.
			CollisionPoint point = CollisionPoint.Retrieve();
#if QUATERNION_ORIENTATION
			point.RelativePosition0 = collisionPosition - shape0.CollisionGroup.Position;
			point.RelativePosition1 = collisionPosition - shape1.CollisionGroup.Position;
#else
#if INLINE
			point.RelativePosition0.X = x - shape0.CollisionGroup.Position.X;
			point.RelativePosition0.Y = y - shape0.CollisionGroup.Position.Y;
			point.RelativePosition0.Z = z - shape0.CollisionGroup.Position.Z;

			point.RelativePosition1.X = x - shape1.CollisionGroup.Position.X;
			point.RelativePosition1.Y = y - shape1.CollisionGroup.Position.Y;
			point.RelativePosition1.Z = z - shape1.CollisionGroup.Position.Z;
#elif REFERENCE
			Vector3.Subtract(ref collisionPosition, ref shape0.CollisionGroup.Position, out point.RelativePosition0);
			Vector3.Subtract(ref collisionPosition, ref shape1.CollisionGroup.Position, out point.RelativePosition1);
#else
			point.RelativePosition0 = collisionPosition - shape0.CollisionGroup.Position;
			point.RelativePosition1 = collisionPosition - shape1.CollisionGroup.Position;
#endif
#endif
			point.Depth = collisionDepth;

			// Add the collision point to the collection.
			CollisionPointCollection collisionPoints = CollisionPointCollection.Retrieve();
			collisionPoints.Add(point);

			// Notify subscribers of the collision.
			context.OnCollided(shape0, shape1, ref collisionNormal, collisionPoints);

			// Recycle the collision point collection.
			CollisionPointCollection.Recycle(collisionPoints);
		}

		#endregion
	}
}
